//
// Created by liuqi on 2025/9/14.
//
#include <robot_manager.h>

void RobotManager::controllerLoop()
{
    ZLOGI("Starting controller loop");
    while (true)
    {
        std::unique_lock<std::mutex> lock(mutex_);
        cv_.wait(lock, [this] { return !move_msg_queue_.empty() || !running_; });
        if (!running_ && move_msg_queue_.empty())
        {
            break;
        }
        if (move_msg_queue_.empty())
        {
            continue;
        }
        auto [type, msg] = move_msg_queue_.front();
        move_msg_queue_.pop();
        lock.unlock();
        // 执行任务
        try
        {
            move_func_map_[type](msg);
        }
        catch (const std::exception &e)
        {
            ZLOGE("执行 {} 任务异常: {}", mapMoveTypeName[type], e.what());
        }
    }
}